/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Reads MaCI Image files (the application of this is for Kinect data handling) and a Position 
* from the files 
* @author Jari Saarinen (jari.saarinen@tkk.fi) - This program is property of AALTO
*/

#include "ImageFileReader.hpp"
#include "owndebug.h"
#include "PositionFileReader.hpp"
#include "3DPointStorage.h"


double testarea=90.0;
double resolution = 0.01;
C3DPointStorage s(testarea+10,testarea+10,testarea+10,resolution);


void printUsage(){
	fprintf(stderr,"\n-----------------------------------------------------------------\n");
	fprintf(stderr,"Reads Image and Position files\n");
	fprintf(stderr,"Usage: ./KinectImagePositionReader OPTS\n");
	fprintf(stderr,"\t[-P] name :: Uses 'name' as MaCI position file\n");
	fprintf(stderr,"\t[-I] name :: Uses 'name' as MaCI ranging file\n");
	fprintf(stderr,"\t[-S] name :: Uses 'name' as Settings file\n");
	
	
	fprintf(stderr,"\nNote that -P and -I must both be given\n");
	fprintf(stderr,"\n'\n");
	fprintf(stderr,"-----------------------------------------------------------------\n\n");
}

static std::vector<float> iDepthVector; ///< Pre-calculated values for depth conversion


//**********************************************************************************
// STRUCTS
struct Vec2i{
  int x, y;

  Vec2i(int aX, int aY) : x(aX), y(aY) {}
  Vec2i(): x(), y() {}
};

struct Vec3f{
  float x,y,z;
  
  Vec3f(float aX, float aY, float aZ) : x(aX), y(aY), z(aZ) {}
  Vec3f() : x(), y(), z() {}

};

struct Matrix4{
  float m00,m01,m02,m10,m11,m12,m20,m21,m22;

  Matrix4(Vec3f aRow1, Vec3f aRow2, Vec3f aRow3) : m00(aRow1.x),
                                                   m01(aRow1.y),
                                                   m02(aRow1.z),
                                                   m10(aRow2.x),
                                                   m11(aRow2.y),
                                                   m12(aRow2.z),
                                                   m20(aRow3.x),
                                                   m21(aRow3.y),
                                                   m22(aRow3.z) {}
  Matrix4() :  m00(),
               m01(),
               m02(),
               m10(),
               m11(),
               m12(),
               m20(),
               m21(),
               m22() {}

  Matrix4 Transpose()
  {
    return Matrix4(Vec3f(m00, m10, m20),
                   Vec3f(m01, m11, m21),
                   Vec3f(m02, m12, m22));

  }


  void Translation(Vec3f aVec)
  {
    m00 += aVec.x;
    m10 += aVec.x;
    m20 += aVec.x;

    m01 += aVec.y;
    m11 += aVec.y;
    m21 += aVec.y;

    m02 += aVec.z;
    m12 += aVec.z;
    m22 += aVec.z;

  }

  Vec3f TransformPoint(const Vec3f aVec)
  {
    return Vec3f(m00*aVec.x + m01*aVec.y + m02*aVec.z,
                 m10*aVec.x + m11*aVec.y + m12*aVec.z,
                 m20*aVec.x + m21*aVec.y + m22*aVec.z);


  }


};


float RawDepthToMeters(int depthValue)
{
  if (depthValue < 2047)
    {
      return float(1.0 / ((double(depthValue) * (-0.0030711016)) + 3.3309495161));
    }
  return 0.0f;

}

//**********************************************************************************
Vec3f DepthToWorld(int x, int y, int depthValue)
{
 
  static const double fx_d = 1.0 / 5.9421434211923247e+02;
  static const double fy_d = 1.0 / 5.9104053696870778e+02;
  static const double cx_d = 3.3930780975300314e+02;
  static const double cy_d = 2.4273913761751615e+02;

  Vec3f result;
  const double depth = iDepthVector.at(depthValue);
  result.x = float((x - cx_d) * depth * fx_d);
  result.y = float((y - cy_d) * depth * fy_d);
  result.z = float(depth);

  return result;  
}



static Matrix4 finalMatrix(
                           Vec3f(9.9984628826577793e-01f,  1.2635359098409581e-03f, -1.7487233004436643e-02f),
                           Vec3f(-1.4779096108364480e-03f, 9.9992385683542895e-01f, -1.2251380107679535e-02f),
                           Vec3f(1.7470421412464927e-02f,  1.2275341476520762e-02f, 9.9977202419716948e-01f));;
static bool MATRIX_INITIALIZED = false;

Vec2i WorldToColor(const Vec3f &pt)
{

  if(MATRIX_INITIALIZED == false){
      static const Vec3f translation(1.9985242312092553e-02f, -7.4423738761617583e-04f, -1.0916736334336222e-02f);
      finalMatrix = finalMatrix.Transpose();
      finalMatrix.Translation(translation);
      MATRIX_INITIALIZED = true;
    }
      
  static const double fx_rgb = 5.2921508098293293e+02;
  static const double fy_rgb = 5.2556393630057437e+02;
  static const double cx_rgb = 3.2894272028759258e+02;
  static const double cy_rgb = 2.6748068171871557e+02;

  const Vec3f transformedPos = finalMatrix.TransformPoint(pt);
  const float invZ = 1.0f / transformedPos.z;

  Vec2i result;
  float tmpx = (transformedPos.x * fx_rgb * invZ) + cx_rgb;
  float tmpy = (transformedPos.y * fy_rgb * invZ) + cy_rgb;
  
  result.x = tmpx;
  result.y = tmpy;
	
  if(result.x < 0)
    result.x = 0;

  if(result.x >= 640)
    result.x = 639;

  if(result.y < 0)
    result.y = 0;

  if(result.y >= 480)
    result.y = 479;

  return result;
}

/**
* Converts from the sensor frame to Global frame using robot position
*/
Vec3f World2Global2D(Vec3f &pBody, float x, float y, float a){
	Vec3f result;
	float xp = pBody.z;
	float yp = -pBody.x;
	float xn = xp*cos(a) - yp*sin(a) + x;
	float yn = xp*sin(a) + yp*cos(a) + y;
	
	result.x = xn;
	result.y = yn;
	result.z = pBody.y;
	return result;
}


/**
* Make conversion
*/
bool processKinectImage(MaCI::Image::CImageData &idata, float xr, float yr, float ar){
	MaCI::Image::CImageContainer depth;
	MaCI::Image::CImageContainer rgb;
	
	// We use a ImageDataIterator to take both data containers
  MaCI::Image::TImageDataIterator dataIte = NULL;
  
	bool r;      
	// We get the images
	r = idata.GetImage(rgb, &dataIte, true);
	fprintf(stderr,"-- retval =%d --", r);
	r &= idata.GetImage(depth, &dataIte, true);

  if(!r){
		fprintf(stderr,"FAILED to parse images!!! \n");
		return false;
	}
        
	depth.ConvertTo(MaCI::Image::KImageDataRGB); ///< aDepth was PNG
  rgb.ConvertTo(MaCI::Image::KImageDataRGB);   ///< aRGB was JPEG

	
	for(size_t i = 0; i < 640; i++){
		for(size_t j = 0; j < 480; j++){
			// We get the original depth value
			int depthR = depth.GetImageDataPtr()[j*640*3 + i*3 + 0];
			int depthB = depth.GetImageDataPtr()[j*640*3 + i*3 + 2];
			int idepth = depthR * 10 + depthB;
			
			// We skip pixels that do not have a valid depth value
			if(idepth < 5 || idepth > 1084) continue;
			if(idepth > 998) continue;
			// We get the world coords (XYZ)
			Vec3f worldvec = DepthToWorld(i, j, idepth);
			
			// We get the 2D coordinates for the color
			Vec2i colorCoords = WorldToColor(worldvec);
			
			const unsigned char *rgbptr = rgb.GetImageDataPtr();
			const int loc = colorCoords.y*640*3 + colorCoords.x*3;
			
			
			unsigned char R = rgbptr[loc + 0];
			unsigned char G = rgbptr[loc + 1];
			unsigned char B = rgbptr[loc + 2];
			
			Vec3f wpos = World2Global2D(worldvec, xr, yr, ar);
			lnm::TCoord3D coord(wpos.x,wpos.y,wpos.z);
			s.setCoordPoint(coord, lnm::RGB(R,G,B));
			// Now we have the XYZRGB data, so we put it in the vector
			//tmpvec.push_back(XYZRGB(worldvec.x, worldvec.y, worldvec.z, r, g, b));
		}
	}
	
	
	
	
	return true;
}




int processFiles(CPositionFileReader &preader, CImageFileReader &ireader){
  std::list<gim::time> times;
  if(ireader.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }
  
  MaCI::Image::CImageData idata;
	MaCI::Position::CPositionData pdata;

	std::list<gim::time>::iterator ii;
	
	MaCI::Position::TPose2D old_pose;
	bool first = true;
	float dl=0,da=0;
	int counter = 0;
	for(ii=times.begin();ii!=times.end();ii++){
		
			if(preader.getClosestTo(pdata,*ii)){
				const MaCI::Position::TPose2D *pose; ///< Pose
				pose = pdata.GetPose2D();
				
				if(pose == NULL){
					fprintf(stderr,"FAILED TO PARSE POSITION!!\n");
					exit(1);
				}
				
				if(!first){
					dl+=sqrt( (pose->x-old_pose.x)*(pose->x-old_pose.x)+ (pose->y-old_pose.y)*(pose->y-old_pose.y));
					da+= fabs(pose->a - old_pose.a);
					old_pose = *pose;
				}else{
					first = false;
					old_pose = *pose;
				}
				
				if(first || da > 15.0 * M_PI / 180.0 || dl > 0.2){
					if(ireader.GetImageData(idata,*ii)){
						fprintf(stderr,"we have %d images at position %.2f %.2f %.2f\n", idata.GetImageCount(),pose->x,pose->y,pose->a);
						processKinectImage(idata,pose->x,pose->y,pose->a);
						dl = 0;
						da = 0;
						counter ++;
					}else{
						fprintf(stderr,"Failed to read the imagefile");
					}
				}
				if(counter > 50){
					fprintf(stderr,"Counter limit exceeded - Will stop\n");
					return 0;
				}
			}else{
					fprintf(stderr,"FAILED TO FIND Position for image\n");
					continue;
			}
			
	}
return 0;
}



int main(int argc, char *argv[]){
  debugInit();
  debugSetGlobalDebugLvl(4);

  char *image_file_name=NULL;
  char *position_file_name=NULL;
  char *settings_file_name = NULL;
  
  ////////////////////////////////////////////////////////////////////////////////////
  // Parse parameters
  ////////////////////////////////////////////////////////////////////////////////////
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "P:I:S:hqv");
    switch(c) {
      
      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
      case 'P': {
	// Take name & minor
	position_file_name = optarg;
	fprintf(stderr,"Using %s as Position file\n",position_file_name);
	break;
      }
      case 'I': {
	image_file_name = optarg;
	fprintf(stderr,"Using %s as Image file\n",image_file_name);
	break;
      }    
      case 'S':
	settings_file_name = optarg;
	dPrint(3,"Using '%s' as settings file name", settings_file_name);
	break;
      case 'h':
      case '?':
	printUsage();
	exit(1);
	break;
	
      case -1:
	parse = false;
	break;
    }
  }
  
  if(image_file_name == NULL || position_file_name == NULL){
    fprintf(stderr,"Not enough inputs given\n");
    printUsage();
    return 0;
  }
  
  CPositionFileReader preader;
  CImageFileReader ireader;
  
  if(preader.ReadFile(position_file_name)){
    dPrint(1,"Succesfully read position file.");
  }else{
    dPrint(1,"Failed to read file position file '%s'", position_file_name);
    exit(1);
  }
  if(ireader.ReadFile(image_file_name)){
    dPrint(1,"Succesfully read image file.");
  }else{
    dPrint(1,"Failed to read file Image file '%s'", image_file_name);
    exit(1);
  }
  
  for(int i = 0; i < 2048; i++){
      iDepthVector.push_back(RawDepthToMeters(i));
  }
  
  processFiles(preader, ireader);
	
	s.save("3Doutput.txt");

return 0;
}